import rcu
class robot:
    speed=100
    now=0
    left=11
    right=12
    both=13
    either=14
    no_line_dis=300
    turn_before_dis=300
    ks=[0.8,0.6,0.3,0,-0.2,-0.5]
leftRed=True
waterTake=True
waterIn2=False
def clearCode():
    rcu.SetMotorCode(1)
    rcu.SetMotorCode(2)
def motors(x1,x2,dis=None,time=None):
    if dis is not None:
        clearCode()
        motors(x1,x2)
        dis=dis*2
        while abs(rcu.GetMotorCode(1))+abs(rcu.GetMotorCode(2))<dis:pass
        motors(0,0)
    elif time is not None:
        motors(x1,x2)
        rcu.SetWaitForTime(time)
        motors(0,0)
    else:
        rcu.SetMotor(1,x1)
        rcu.SetMotor(2,x2)
def getTrace(index=None):
    x=rcu.GetTraceV2I2C(1,9)
    if index is None:
        a = [0] * 8
        a[7]=x
        for i in range(7):
            a[i]=x>>i&1
        return a
    else:
        if 0<=index<7:return x>>index&1
        else:return -1
def limit(x):
    if x>100:return 100
    if x<-100:return -100
    else:return x
def goDis(dis=300,v=80,k=0.4,mode=robot.now,outdis=0,time=None):
    rcu.SetMotorCode(1)
    rcu.SetMotorCode(2)
    dis=dis*2
    d1=d2=0
    rcu.SetSysTime()
    v=int(robot.speed*v/100)
    while True: 
        a=getTrace()  
        d1=rcu.GetMotorCode(1)
        d2=rcu.GetMotorCode(2)
        if abs(d1+d2)>dis:
            if mode==robot.now:break
            elif mode==robot.left:
                if a[0]:break             
            elif mode==robot.right:
                if a[6]:break
            elif mode==robot.either:
                if a[0] or a[6]:break
            elif mode==robot.both:
                if a[0] and a[6]:break 
            if a[0] or a[6]:break
        dv=(d1-d2)*k
        v1=limit(v-dv)
        v2=limit(v+dv)
        motors(v1,v2)
        if time!=None: 
             if rcu.GetSysTime()>time:
                 break
            
    stopBeep()
def goLine(dis=300,mode=robot.now,v=90,outdis=500,time=None):
    clearCode()
    #SetFontSize(2)
    d1=0
    v1=v
    dis=dis*2
    v=robot.speed*v/100
    kid=0
    def md(x,y):
        motors(v*robot.ks[x],v*robot.ks[y])
    motors(v,v)
    while True:
        d=rcu.GetMotorCode(1)+rcu.GetMotorCode(2)
        #SetDisplayVar(1,d/2,9999,0)
        a=getTrace()         
        if d>dis:
            if mode==robot.now:break
            elif mode==robot.left:
                if a[1]:break             
            elif mode==robot.right:
                if a[6]:break
            elif mode==robot.either:
                if a[2] or a[6]:break 
            elif mode==robot.both:
                if a[2] and a[6]:break          
        if a[7]>0:
            d1=d
            if a[7]>130:
                rcu.SetInBeep(1)
                motors(0,0)            
            elif a[3]:
                if a[2]==a[4]:md(0,0)
                elif a[2]:md(1,0)
                else:md(0,1)
            elif a[2]:
                if a[1]:md(3,0)
                else:md(2,0)
            elif a[4]:
                if a[4]:md(0,3)
                else:md(0,2)
            elif a[1]:
                md(4,0)
            elif a[5]:
                md(0,4)
            elif a[0]:
                if a[6]:md(0,0)
                else:
                    md(5,0)
                    kid=-1
            elif a[6]:
                md(0,5)
                kid=1
            else:md(0,0)
        else:
            if d-d1>robot.no_line_dis:
                if kid<0:motors(-v,v)
                elif kid>0:motors(v,-v)
                else:md(0,0)
            #else:md(0,0)
    if mode!=robot.now:goDis(outdis,v1)
    else:
        motors(0,0)
    
        stopBeep()
def turnLeft(v=50,dis=robot.turn_before_dis):
    clearCode()
    motors(-v,v)
    dis=dis*2
    while abs(rcu.GetMotorCode(1))+abs(rcu.GetMotorCode(2))<dis:pass
    while getTrace(2)==1:pass
    while getTrace(2)==0:pass
    motors(0,0)
def turnRight(v=50,dis=robot.turn_before_dis):
    clearCode()
    motors(v,-v)
    dis=dis*2
    while abs(rcu.GetMotorCode(1))+abs(rcu.GetMotorCode(2))<dis:pass
    while getTrace(4)==1:pass
    while getTrace(4)==0:pass
    motors(0,0)
def stopBeep(time=0.03):
    motors(0,0)
    rcu.SetInBeep(1)
    rcu.SetWaitForTime(time)
    rcu.SetInBeep(0)
def motorsN(v=100,time=None,dis=None):
    clearCode()
    rcu.SetMotor(3,v)
    rcu.SetMotor(4,v)
    if time is not None:
        rcu.SetWaitForTime(time)
    else:
        dis*=2
        while abs(rcu.GetMotorCode(3))+abs(rcu.GetMotorCode(4))<dis:pass
    rcu.SetMotor(3,0)
    rcu.SetMotor(4,0)
def handAngle(ins=None,out=None,time=0.6):
    if ins:rcu.SetMagneticServoDegreeTime(3,ins,100)
    if out:rcu.SetMagneticServoDegreeTime(4,out,100)
    if ins or out:rcu.SetWaitForTime(time)
def motorServo(speed=0,angle=0):
    rcu.SetMotorServo(3,speed,angle)
    rcu.SetWaitForTime(0.5)
def motor3init():
    rcu.SetMotor(3,-50)
    rcu.SetWaitForTime(0.5)
    rcu.SetMotor(3,0)

def shuaka():
    handAngle(180,200)
    goLine(1000,mode=robot.right,outdis=0)
    goDis(400,-40)
    motors(0,-50,300)
    motorServo(70,900)
motor3init()
handAngle(155,210)
while rcu.GetTouchScreen()==0:pass
goDis(3500,v=50)
handAngle(120,160,time=0.8)
goDis(1000,v=-50)
motors(50,-50,dis=350)
handAngle(170,190,time=1)
goDis(1500,v=50)
handAngle(130,160,time=1)
goDis(1700,v=-50)
motors(50,-50,dis=660)
goDis(1000,v=70)
goLine(800,mode=robot.right)
goLine(800,mode=robot.right)
goLine(800,mode=robot.right)
goLine(mode=robot.right)
handAngle(130,out=210)
motors(-50,50,dis=500)
goDis(2000)
goDis(2000,v=-80)
handAngle(out=160)
turnLeft()
goLine(mode=robot.right)
turnLeft()
goLine(mode=robot.left)
motors(50,-50,dis=200)
handAngle(180,210)
goDis(3000)
goDis(3000,v=-80)
turnRight()
turnRight()
handAngle(150,210)
motorServo(70,900)
goLine(1400)
motorServo(-70,140)
goLine(600,v=50)
handAngle(130)
goDis(2200,v=-50)
turnRight(dis=100)
motors(50,-50,dis=800)
handAngle(180,210)
goDis(3000)
goDis(3000,v=-80)
turnRight()
goLine(mode=robot.right)
turnLeft()
goLine(mode=robot.left,outdis=1500)
motor3init()
handAngle(125,145)
motors(-60,0,dis=1050)#背对4号位置的 转弯角度
goDis(2000,v=-50)
handAngle(150,210)
motorServo(70,900)
goDis(2000,v=50)
motorServo(-70,130)
goLine(600,v=60)# 抓取第二个房子里
handAngle(130)
goDis(1000,v=-50)
motors(0,50,dis=900)
goLine(1000,mode=robot.right)
goLine(mode=robot.right)
motors(-50,50,dis=500)
handAngle(180)
goDis(2000)
goDis(2000,v=-60)
turnLeft(dis=700)
handAngle(125,150)
motor3init()
goLine(mode=robot.right)
goLine(mode=robot.left)
goLine(mode=robot.left,outdis=200)
motors(-70,0,dis=1100)
goDis(2000,v=-50)
goLine(1000,mode=robot.right,v=50)
motors(60,0,dis=1000)
motors(0,60,dis=1100)#背推1号位后路口往右平移。调整距离，正对线
handAngle(155,210)
goLine(2500)
v=[60,0]
if leftRed:v=[0,60]
motors(v[0],v[1],200)
goDis(1000,v=50)
handAngle(125,150)
goDis(1000,v=-50)
dis1=0
if leftRed:dis1=-400
motors(-50,50,dis=dis1+800)
handAngle(180,190)
goDis(2000,v=70)
goDis(2000,v=-50)
turnLeft()
goLine(mode=robot.left,outdis=0)
goDis(1500,v=-50)
motors(-50,50,dis=1100)
goDis(1500)
handAngle(130,160)
goDis(1500,v=-50)
turnRight()
goLine(mode=robot.right,outdis=0)
goDis(200,v=-50)
motors(50,0,dis=1000)
goLine(mode=robot.right)
goLine(mode=robot.right)
turnLeft()
handAngle(180,190)
goLine(mode=robot.left,outdis=300,v=50)
goDis(1000,v=-50)
turnRight()
goLine(mode=robot.right)
motors(-50,50,dis=600)
handAngle(130,210)
goDis(2000,v=100)
goDis(2000,v=-50)
turnLeft(dis=700)
handAngle(180,210)
goLine(mode=robot.right)

if waterTake:#要做水塔任务
    goLine(mode=robot.left)
    goLine(mode=robot.right,outdis=0)
    goDis(500,v=-50)
    motors(0,-50,dis=1100)
    handAngle(180,210)
    motor3init()
    goDis(300,v=50)
    motorServo(40,750)
    goDis(800,v=-50)
    rcu.SetWaitForTime(0.5)
    goDis(800,v=50)
    motor3init()
    handAngle(130,210,2)
    goDis(300,v=-50)
    handAngle(130,160)
    turnLeft()
    goLine(mode=robot.right)
    turnRight()
    handAngle(180,210)
    goLine(mode=robot.left)
    goDis(1000,v=-50)
    if not waterIn2:
        handAngle(130,160)
        motors(0,50,dis=1000)
        motors(50,0,dis=1000)
        goDis(1000,v=-50)
    turnRight()
    goLine(mode=robot.right,outdis=0)
    goDis(200,v=-50)#修正左右位置
    motors(-50,0,dis=1000)
    handAngle(180,210)
    motor3init()
    goDis(300,v=50)
    motorServo(40,750)
    goDis(800,v=-50)
    rcu.SetWaitForTime(0.5)
    goDis(800,v=50)
    motor3init()
    handAngle(130,210,2)
    goDis(300,v=-50)
    handAngle(130,160)
    if waterIn2:
        turnLeft()
        goLine(mode=robot.right)
        goLine(mode=robot.right)
        turnRight()
        handAngle(180,210)
        goLine(1200,mode=robot.right,v=50,outdis=0)
        goDis(1500,v=-50)
        turnRight()
        goLine(mode=robot.right)
        goLine(mode=robot.right,outdis=0)
        goDis(900,v=-50)
        handAngle(130,160)
        turnLeft()
        goLine(500,mode=robot.right,outdis=0)       
        goDis(1100,v=-50) 
    else:
        turnRight()
        goLine(2500,mode=robot.right,outdis=0)
        goDis(900,v=-50)
        turnLeft()
        handAngle(180,200)
        goLine(600,mode=robot.right,outdis=300)
        goDis(1100,v=-50)
    handAngle(180,210)
    turnRight()
    goLine(1500,mode=robot.right)
    goDis(1500,v=-50)
    turnRight()


else:
    turnLeft()
goLine(mode=robot.right,outdis=0)
goDis(700,v=-50)#刷卡前后误差
motor3init()
motors(50,-50,dis=200)#刷卡 左右误差
motorServo(70,900)
def task1():pass
def task2():pass
def task3():pass
def task4():pass
def mutiTask():
    rcu.SetLCDFilledRectangle2(0,0,120,160,0x00007f)
    rcu.SetLCDFilledRectangle2(120,0,240,160,0x7f0000)
    rcu.SetLCDFilledRectangle2(0,160,120,320,0x007f00)
    rcu.SetLCDFilledRectangle2(120,160,240,320,0x00ffff)
    while True:
        while rcu.GetTouchScreen()==0:pass
        x=rcu.GetTouchScreenX()
        y=rcu.GetTouchScreenY()
        if x<120:
            if y<160: task1()
            else: task3()
        else:
            if y<160: task2()
            else: task4()